/*
 * @Description: 
 * @Author: Niezl
 * @Gitee: https://gitee.com/Morphlng
 * @Date: 2021-04-07 08:08:09
 * @LastEditors: Niezl
 * @LastEditTime: 2021-04-07 22:55:34
 */
#include <vector>
#include <functional>
#include <set>
#include <algorithm>

using uint = unsigned int;

// 二维坐标定义
struct Vec2
{
    int x, y;
    Vec2() {}
    Vec2(int _x, int _y)
    {
        x = _x;
        y = _y;
    }

    bool operator==(const Vec2 &coord)
    {
        return (x == coord.x && y == coord.y);
    }

    bool operator!=(const Vec2 &coord)
    {
        return *this == coord;
    }

    Vec2 operator+(const Vec2 &coord)
    {
        return {x + coord.x, y + coord.y};
    }
};
using CoordinateList = std::vector<Vec2>;

// 每一个方格是一个节点
struct Node
{
    // G = 从起点 A 移动到指定方格的移动代价，沿着到达该方格而生成的路径。
    uint G;
    // H = 从指定的方格移动到终点 B 的估算成本。
    uint H;
    // 结点坐标
    Vec2 coordinates;
    // 父节点，表示当前节点的F=G+H是由哪个结点计算出来的
    Node *parent;

    Node(Vec2 _coord, Node *_parent = nullptr)
    {
        parent = _parent;
        coordinates = _coord;
        G = H = 0;
    }

    uint getScore()
    {
        return G + H;
    }
};
using NodeSet = std::vector<Node *>;

class Map
{
    bool detectCollision(Vec2 coord);
    Node *findNodeOnList(NodeSet &nodes, Vec2 coord);
    void releaseNodes(NodeSet &nodes);

public:
    Map();
    void setWorldSize(Vec2);
    // 启用对角方向移动
    void setDiagonalMovement(bool);
    // A*算法找到最佳路径
    CoordinateList findPath(Vec2 source, Vec2 target);
    // 添加障碍物
    void addCollision(Vec2);
    // 删除障碍物
    void removeCollision(Vec2);
    // 清楚障碍物
    void clearCollisions();

private:
    CoordinateList direction; // 可移动方向(上下左右，或包括对角方向)
    CoordinateList walls;     // 障碍物集合
    Vec2 worldSize;           // 地图大小
    uint directions;          // 4：上下左右；8：包括对角
};

#pragma region 基础成员操作
Map::Map()
{
    setDiagonalMovement(false);
    direction = {
        Vec2(0, 1), Vec2(1, 0), Vec2(0, -1), Vec2(-1, 0),  // 上下左右
        Vec2(-1, -1), Vec2(1, 1), Vec2(-1, 1), Vec2(1, -1) // 对角方向
    };
}

void Map::setWorldSize(Vec2 _worldSize)
{
    worldSize = _worldSize;
}

void Map::setDiagonalMovement(bool _enable)
{
    directions = (_enable ? 8 : 4);
}

void Map::addCollision(Vec2 _coordinates)
{
    walls.push_back(_coordinates);
}

void Map::removeCollision(Vec2 _coordinates)
{
    CoordinateList::iterator it = std::find(walls.begin(), walls.end(), _coordinates);
    if (it != walls.end())
        walls.erase(it);
}

void Map::clearCollisions()
{
    walls.clear();
}
#pragma endregion

#pragma region A_Star
bool Map::detectCollision(Vec2 coord)
{
    // 越界或碰到障碍物，则发生碰撞
    if (coord.x < 0 || coord.x >= worldSize.x ||
        coord.y < 0 || coord.y >= worldSize.y ||
        std::find(walls.begin(), walls.end(), coord) != walls.end())
        return true;
    return false;
}

Node *Map::findNodeOnList(NodeSet &nodes, Vec2 coord)
{
    for (auto node : nodes)
    {
        if (node->coordinates == coord)
        {
            return node;
        }
    }
    return nullptr;
}

void Map::releaseNodes(NodeSet &nodes)
{
    for (auto it = nodes.begin(); it != nodes.end();)
    {
        delete *it;
        it = nodes.erase(it);
    }
}

// 返回最短路径节点集合
CoordinateList Map::findPath(Vec2 source, Vec2 target)
{
    Node *current = nullptr;
    NodeSet openSet, closedSet;
    openSet.reserve(100);
    closedSet.reserve(100);
    openSet.push_back(new Node(source));

    while (!openSet.empty())
    {
        auto current_it = openSet.begin();
        current = *current_it;

        // 找到F值（代价）最小的点
        for (auto it = openSet.begin(); it != openSet.end(); it++)
        {
            auto node = *it;
            if (node->getScore() <= current->getScore())
            {
                current = node;
                current_it = it;
            }
        }

        if (current->coordinates == target)
        {
            break;
        }

        closedSet.push_back(current);
        openSet.erase(current_it);

        // 检查新加入结点A相邻的方格，把可达(Reachable)的加入到openList，设A为它们的父结点
        for (uint i = 0; i < directions; ++i)
        {
            Vec2 newCoordinates(current->coordinates + direction[i]);

            // 检测到碰撞或该结点存在在closeSet中，则跳过
            if (detectCollision(newCoordinates) ||
                findNodeOnList(closedSet, newCoordinates))
                continue;

            // 上下左右取代价为10，对角线代价为14
            uint totalCost = current->G + ((i < 4) ? 10 : 14);

            Node *successor = findNodeOnList(openSet, newCoordinates);
            if (successor == nullptr)
            {
                successor = new Node(newCoordinates, current);
                successor->G = totalCost;

                Vec2 delta(abs(successor->coordinates.x - target.x), abs(successor->coordinates.y - target.y));
                successor->H = 10 * (delta.x + delta.y);

                openSet.push_back(successor);
            }
            else if (totalCost < successor->G)
            {
                successor->parent = current;
                successor->G = totalCost;
            }
        }
    }

    CoordinateList path; // 由于从终点开始加入结果，因此保存的结果为逆序
    if (current->coordinates == target)
        while (current != nullptr)
        {
            path.push_back(current->coordinates);
            current = current->parent;
        }

    releaseNodes(openSet);
    releaseNodes(closedSet);

    return path;
}
#pragma endregion